#include <vector>
#include <map>
#include <string>
#include <functional>

using namespace std;

//******************************
//          结构体定义
//******************************
    /*struct: Pose
     * 表示位置的结构体
     * 1 左上角为原点，横向为x，纵向为y
     */
    struct Pose
    {
        Pose(){}
        Pose(double _x, double _y, int _id){x=_x; y=_y; id=_id;}

        double x = 0;     //位置x
        double y = 0;     //位置y
        int   id = 0;       
    };

    /*struct: Vertex/Edge
     * 表示道路相关信息的结构体
     * 1 用于表示道路的顶点、边
     */
    struct Vertex
    {
        Vertex(){}
        Vertex(vector<int> related_vertexes, vector<int> related_edges, Pose vertex_pos)
        {
            relatedVertexes = related_vertexes;
            relatedEdges    = related_edges;
            vertexPos         = vertex_pos;
        }

        vector<int> relatedVertexes;        //与该顶点关联的所有顶点和对应的边的ID，是否关联，根据车道标志（左转等）确定
        vector<int> relatedEdges;
        Pose vertexPos;                 //该顶点的位置和ID
    };

    struct Edge
    {
        Edge(){}
        Edge(int id, vector<int> related_vertexes, int type,
                    double dis, double angle, int cost, bool isReverse = false)
        {
            relatedVertexes.clear();
            relatedVertexes = related_vertexes;
            
            attr_id         = id;
            attr_type       = type;
            attr_dis        = dis;
            attr_angle      = angle;
            attr_cost       = cost;
            attr_isReverse  = isReverse;
        }

        vector<int> relatedVertexes;        //该道路两端的顶点ID
        int attr_id;                        //边属性0：id
        int attr_type;                      //边属性1：1代表直行，2代表左转弯，3代表右转弯
        double attr_dis;                    //边属性2：直行时代表路段长度，转弯时代表路段半径，单位：mm
        double attr_angle;                  //边属性3：直行时取0，转弯时取转弯的角度，单位：度
        int attr_cost;                      //边属性4：计算最短路径时使用的代价值
        bool attr_isReverse;                //边属性5：入库时，是否属于倒车边，车库外的边默认false
    };


    
//******************************
//          全局变量
//******************************
//定义车辆信息
    vector<string> vehicleInfoS{"car","car","car","car","car","bus","bus","bus","van","truck"};
 
//定义信号灯信息   
    //                              {xmin,xmax,ymin,ymax}
    vector<vector<double>> TLInfoS{ {5030,5230,0,214},
                                    {4058,4272,752,992},
                                    {3036,3336,226,660},
                                    {226,440,3731,3931},
                                    {0,214,2467,2667},
                                    {2717,2916,2979,3193},
                                    {1044,1244,3205,3419},
                                    {5886,6100,3731,3931},
                                    {5660,5874,1421,1621},
                                    {4810,5010,4610,4824},
                                    {3066,3266,4836,5050} }; 
    
//定义拓扑地图的顶点  
    //                              { {可到达的顶点}, {对应的边},  Pose{x,y,id}}
    vector<Vertex> mapVertexInfoS{  { {2}, {1}, Pose(720,107,1) },
                                    { {3}, {2}, Pose(107,720,2) },
                                    { {4,5}, {3,4}, Pose(107,2699,3) },
                                    { {6}, {5}, Pose(107,4330,4) },
                                    { {7}, {6}, Pose(720,3313,5) },
                                    { {8}, {7}, Pose(720,4943,6) },
                                    { {9,10}, {8,9}, Pose(1710,3312,7) },
                                    { {11}, {10}, Pose(2329,4943,8) },
                                    { {12}, {11}, Pose(1979,3430,9) },
                                    { {13,14}, {12,13}, Pose(1983,3312,10) },
                                    { {15}, {14}, Pose(3298,4943,11) },
                                    { {16}, {15}, Pose(2265,3725,12) },
                                    { {17}, {16}, Pose(2262,3430,13) },
                                    { {18}, {17}, Pose(3142,3312,14) },
                                    { {19}, {18}, Pose(5380,4943,15) },
                                    { {20}, {19}, Pose(1760,3204,16) },
                                    { {21}, {20}, Pose(2547,3725,17) },
                                    { {22}, {21}, Pose(3471,3499,18) },
                                    { {23}, {22}, Pose(5993,4322,19) },
                                    { {24}, {23}, Pose(1482,3086,20) },
                                    { {25}, {24}, Pose(2043,3204,21) },
                                    { {26,27}, {25,26}, Pose(3576,3644,22) },
                                    { {28,29}, {27,28}, Pose(5993,3699,23) },
                                    { {30}, {29}, Pose(1244,3086,24) },
                                    { {20}, {30}, Pose(1765,3086,25) },
                                    { {31}, {31}, Pose(3683,3910,26) },
                                    { {32}, {32}, Pose(4501,3644,27) },
                                    { {33}, {33}, Pose(5380,3086,28) },
                                    { {34}, {34}, Pose(5993,2699,29) },
                                    { {35}, {35}, Pose(720,3086,30) },
                                    { {36}, {36}, Pose(3685,4330,31) },
                                    { {37,38}, {37,38}, Pose(4599,3511,32) },
                                    { {38}, {39}, Pose(4935,3083,33) },
                                    { {39,40}, {40,41}, Pose(5993,2653,34) },
                                    { {41}, {42}, Pose(333,2699,35) },
                                    { {42}, {43}, Pose(3298,4717,36) },
                                    { {43}, {44}, Pose(4935,3314,37) },
                                    { {44}, {45}, Pose(4599,2886,38) },
                                    { {45}, {46}, Pose(5380,2040,39) },
                                    { {46}, {47}, Pose(5993,724,40) },
                                    { {47,48}, {48,49}, Pose(333,2574,41) },
                                    { {49,50}, {50,51}, Pose(2350,4717,42) },
                                    { {29,51}, {52,53}, Pose(5380,3312,43) },
                                    { {52,53}, {54,55}, Pose(4501,2754,44) },
                                    { {54}, {56}, Pose(5262,2040,45) },
                                    { {55}, {57}, Pose(5380,107,46) },
                                    { {56}, {58}, Pose(516,1912,47) },
                                    { {57}, {59}, Pose(320,667,48) },
                                    { {58}, {60}, Pose(1960,4330,49) },
                                    { {59}, {61}, Pose(1329,4717,50) },
                                    { {60}, {62}, Pose(5767,3699,51) },
                                    { {61}, {63}, Pose(4394,2487,52) },
                                    { {62}, {64}, Pose(3576,2754,53) },
                                    { {63}, {65}, Pose(4864,1642,54) },
                                    { {64,65}, {66,67}, Pose(4260,107,55) },
                                    { {66}, {68}, Pose(592,1579,56) },
                                    { {67}, {69}, Pose(758,112,57) },
                                    { {68}, {70}, Pose(1960,4256,58) },
                                    { {69}, {71}, Pose(720,4717,59) },
                                    { {70}, {72}, Pose(5767,4330,60) },
                                    { {71,72,73}, {73,74,75}, Pose(4391,1764,61) },
                                    { {18,74}, {76,77}, Pose(3478,2886,62) },
                                    { {75}, {78}, Pose(4864,902,63) },
                                    { {76}, {79}, Pose(3685,682,64) },
                                    { {1}, {80}, Pose(3545,107,65) },
                                    { {77}, {81}, Pose(588,705,66) },
                                    { {78}, {82}, Pose(1059,101,67) },
                                    { {79}, {83}, Pose(1223,3166,68) },
                                    { {80}, {84}, Pose(333,4330,69) },
                                    { {81}, {85}, Pose(5380,4717,70) },
                                    { {82}, {86}, Pose(4388,720,71) },
                                    { {83}, {87}, Pose(4274,1410,72) },
                                    { {84}, {88}, Pose(4501,1419,73) },
                                    { {85}, {89}, Pose(3142,3083,74) },
                                    { {86}, {90}, Pose(4167,205,75) },
                                    { {87}, {91}, Pose(3685,1753,76) },
                                    { {88,89}, {92,93}, Pose(879,415,77) },
                                    { {90}, {94}, Pose(1369,276,78) },
                                    { {91}, {95}, Pose(648,2326,79) },
                                    { {5,35}, {96,97}, Pose(333,3699,80) },
                                    { {36,92}, {98,99}, Pose(4778,4717,81) },
                                    { {93}, {100}, Pose(4775,333,82) },
                                    { {94}, {101}, Pose(4158,1092,83) },
                                    { {95}, {102}, Pose(4612,1073,84) },
                                    { {25,96}, {103,104}, Pose(2611,3085,85) },
                                    { {97}, {105}, Pose(3880,204,86) },
                                    { {53}, {106}, Pose(3684,2487,87) },
                                    { {98}, {107}, Pose(1169,705,88) },
                                    { {99,100}, {108,109}, Pose(1303,415,89) },
                                    { {101}, {110}, Pose(1292,871,90) },
                                    { {102}, {111}, Pose(729,1935,91) },
                                    { {103}, {112}, Pose(4391,4330,92) },
                                    { {104}, {113}, Pose(5000,333,93) },
                                    { {65}, {114}, Pose(4158,720,94) },
                                    { {93}, {115}, Pose(4612,720,95) },
                                    { {150}, {174}, Pose(2340,2814,96) },
                                    { {105}, {116}, Pose(3250,406,97) },
                                    { {106,107}, {117,118}, Pose(1169,840,98) },
                                    { {108}, {119}, Pose(1520,374,99) },
                                    { {109}, {120}, Pose(1583,484,100) },
                                    { {110}, {121}, Pose(994,1119,101) },
                                    { {111,112}, {122,123}, Pose(1037,1418,102) },
                                    { {27}, {124}, Pose(4394,3910,103) },
                                    { {113}, {125}, Pose(5380,333,104) },
                                    { {114}, {126}, Pose(2794,728,105) },
                                    { {0}, {0}, Pose(1169,1589,106) },
                                    { {115}, {127}, Pose(1269,1155,107) },
                                    { {116}, {128}, Pose(1738,333,108) },
                                    { {117}, {129}, Pose(1863,553,109) },
                                    { {118}, {130}, Pose(845,1268,110) },
                                    { {119}, {131}, Pose(1815,1277,111) },
                                    { {120}, {132}, Pose(1669,1107,112) },
                                    { {121}, {133}, Pose(5767,724,113) },
                                    { {122}, {134}, Pose(4393,856,114) },
                                    { {123}, {135}, Pose(1470,1438,115) },
                                    { {64,124}, {136,137}, Pose(3336,333,116) },
                                    { {125}, {138}, Pose(2305,553,117) },
                                    { {126}, {139}, Pose(504,1842,118) },
                                    { {127}, {140}, Pose(2101,1357,119) },
                                    { {128}, {141}, Pose(2394,1100,120) },
                                    { {39,129}, {142,143}, Pose(5767,1653,121) },
                                    { {101}, {144}, Pose(1662,863,122) },
                                    { {130}, {145}, Pose(1524,1610,123) },
                                    { {82}, {146}, Pose(3659,333,124) },
                                    { {131}, {147}, Pose(2751,553,125) },
                                    { {132}, {148}, Pose(404,2326,126) },
                                    { {76}, {149}, Pose(3298,1366,127) },
                                    { {133}, {150}, Pose(2935,927,128) },
                                    { {134}, {151}, Pose(5767,2675,129) },
                                    { {24}, {152}, Pose(1524,2815,130) },
                                    { {135}, {153}, Pose(2967,553,131) },
                                    { {136}, {154}, Pose(1134,3395,132) },
                                    { {137}, {155}, Pose(3390,605,133) },
                                    { {28,51}, {156,157}, Pose(5767,2699,134) },
                                    { {124}, {158}, Pose(3313,443,135) },
                                    { {138}, {159}, Pose(1716,4254,136) },
                                    { {139}, {160}, Pose(3880,449,137) },
                                    { {8,50}, {161,162}, Pose(1716,4330,138) },
                                    { {140}, {163}, Pose(4167,449,139) },
                                    { {141}, {164}, Pose(4620,902,140) },
                                    { {142}, {165}, Pose(4620,1642,141) },
                                    { {143}, {166}, Pose(5262,2284,142) },
                                    { {129}, {167}, Pose(5380,2284,143) },
                                    { {145}, {168}, Pose(2121,1644,144) },
                                    { {146}, {169}, Pose(1726,1407,145) },
                                    { {147}, {170}, Pose(1490,962,146) },
                                    { {148}, {171}, Pose(1457,831,147) },
                                    { {109}, {172}, Pose(1735,553,148) },
                                    { {130}, {173}, Pose(1342,2420,149) },
                                    { {0}, {0}, Pose(2340,1560,150) }, 
                                    { {152}, {175}, Pose(2437,1812,151) }, 
                                    { {144}, {176}, Pose(2394,1764,152) } };

//定义拓扑地图的边    
    //                              { id, {起点，终点}，边属性1，边属性2，边属性3，边属性4 } 
    vector<Edge> mapEdgeInfoS {      { 1, {1,2}, 2, 613, 90, 963 },
                                    { 2, {2,3}, 1, 1979, 0, 1979 },
                                    { 3, {3,4}, 1, 1631, 0, 1631 },
                                    { 4, {3,5}, 2, 613, 90, 963 },
                                    { 5, {4,6}, 2, 613, 90, 963 },
                                    { 6, {5,7}, 1, 981, 0, 981 },
                                    { 7, {6,8}, 1, 1609, 0, 1609 },
                                    { 8, {7,9}, 3, 387, 46, 311 },
                                    { 9, {7,10}, 1, 283, 0, 283 },
                                    { 10, {8,11}, 1, 969, 0, 969 },
                                    { 11, {9,12}, 1, 411, 0, 411 },
                                    { 12, {10,13}, 3, 387, 46, 311 },
                                    { 13, {10,14}, 1, 1158, 0, 1158 },
                                    { 14, {11,15}, 1, 2082, 0, 2082 },
                                    { 15, {12,16}, 1, 725, 0, 725 },
                                    { 16, {13,17}, 1, 411, 0, 411 },
                                    { 17, {14,18}, 3, 395, 57, 393 },
                                    { 18, {15,19}, 2, 613, 90, 963 },
                                    { 19, {16,20}, 2, 387, 46, 311 },
                                    { 20, {17,21}, 1, 725, 0, 725 },
                                    { 21, {18,22}, 2, 642, 16, 179 },
                                    { 22, {19,23}, 1, 631, 0, 631 },
                                    { 23, {20,24}, 1, 238, 0, 238 },
                                    { 24, {21,25}, 2, 387, 46, 311 },
                                    { 25, {22,26}, 3, 385, 44, 296 },
                                    { 26, {22,27}, 2, 642, 92, 1031 },
                                    { 27, {23,28}, 2, 613, 90, 963 },
                                    { 28, {23,29}, 1, 1000, 0, 1000 },
                                    { 29, {24,30}, 1, 524, 0, 524 },
                                    { 30, {25,20}, 1, 283, 0, 283 },
                                    { 31, {26,31}, 1, 420, 0, 420 },
                                    { 32, {27,32}, 2, 642, 15, 168 },
                                    { 33, {28,33}, 1, 445, 0, 445 },
                                    { 34, {29,34}, 1, 46, 0, 46 },
                                    { 35, {30,35}, 3, 387, 90, 608 },
                                    { 36, {31,36}, 3, 387, 90, 608 },
                                    { 37, {32,37}, 3, 385, 61, 410 },
                                    { 38, {32,38}, 2, 642, 58, 650 },
                                    { 39, {33,38}, 3, 385, 61, 410 },
                                    { 40, {34,39}, 2, 613, 90, 963 },
                                    { 41, {34,40}, 1, 1929, 0, 1929 },
                                    { 42, {35,41}, 1, 124, 0, 124 },
                                    { 43, {36,42}, 1, 948, 0, 948 },
                                    { 44, {37,43}, 1, 445, 0, 445 },
                                    { 45, {38,44}, 2, 642, 15, 168 },
                                    { 46, {39,45}, 1, 118, 0, 118 },
                                    { 47, {40,46}, 2, 613, 90, 963 },
                                    { 48, {41,47}, 3, 1907, 21, 699 },
                                    { 49, {41,48}, 1, 1907, 0, 1907 },
                                    { 50, {42,49}, 3, 387, 90, 608 },
                                    { 51, {42,50}, 1, 1021, 0, 1021 },
                                    { 52, {43,29}, 2, 613, 90, 963 },
                                    { 53, {43,51}, 3, 387, 90, 608 },
                                    { 54, {44,52}, 3, 385, 44, 296 },
                                    { 55, {44,53}, 2, 642, 92, 1031 },
                                    { 56, {45,54}, 3, 398, 90, 625 },
                                    { 57, {46,55}, 1, 1120, 0, 1120 },
                                    { 58, {47,56}, 2, 757, 26, 344 },
                                    { 59, {48,57}, 3, 580, 75, 759 },
                                    { 60, {49,58}, 1, 75, 0, 75 },
                                    { 61, {50,59}, 1, 609, 0, 609 },
                                    { 62, {51,60}, 1, 631, 0, 631 },
                                    { 63, {52,61}, 1, 722, 0, 722 },
                                    { 64, {53,62}, 2, 642, 15, 168 },
                                    { 65, {54,63}, 1, 740, 0, 740 },
                                    { 66, {55,64}, 2, 575, 90, 903 },
                                    { 67, {55,65}, 1, 715, 0, 715 },
                                    { 68, {56,66}, 1, 874, 0, 874 },
                                    { 69, {57,67}, 3, 854, 20, 298 },
                                    { 70, {58,68}, 2, 1150, 70, 1405 },
                                    { 71, {59,69}, 3, 387, 90, 608 },
                                    { 72, {60,70}, 3, 387, 90, 608 },
                                    { 73, {61,71}, 1, 1046, 0, 1046 },
                                    { 74, {61,72}, 2, 600, 36, 377 },
                                    { 75, {61,73}, 3, 600, 35, 367 },
                                    { 76, {62,18}, 2, 642, 57, 639 },
                                    { 77, {62,74}, 3, 385, 61, 410 },
                                    { 78, {63,75}, 2, 697, 90, 1095 },
                                    { 79, {64,76}, 1, 1071, 0, 1071 },
                                    { 80, {65,1}, 1, 2825, 0, 2825 },
                                    { 81, {66,77}, 3, 291, 90, 457 },
                                    { 82, {67,78}, 3, 456, 46, 366 },
                                    { 83, {68,79}, 3, 905, 68, 1074 },
                                    { 84, {69,80}, 1, 631, 0, 631 },
                                    { 85, {70,81}, 1, 602, 0, 602 },
                                    { 86, {71,82}, 3, 387, 90, 608 },
                                    { 87, {72,83}, 3, 600, 33, 346 },
                                    { 88, {73,84}, 2, 600, 35, 367 },
                                    { 89, {74,85}, 1, 531, 0, 531 },
                                    { 90, {75,86}, 1, 287, 0, 287 },
                                    { 91, {76,87}, 1, 734, 0, 734 },
                                    { 92, {77,88}, 3, 291, 90, 457 },
                                    { 93, {77,89}, 1, 424, 0, 424 },
                                    { 94, {78,90}, 3, 439, 86, 659 },
                                    { 95, {79,91}, 3, 1055, 22, 405 },
                                    { 96, {80,5}, 3, 387, 90, 608 },
                                    { 97, {80,35}, 1, 1000, 0, 1000 },
                                    { 98, {81,36}, 1, 1480, 0, 1480 },
                                    { 99, {81,92}, 3, 387, 90, 608 },
                                    { 100, {82,93}, 1, 225, 0, 225 },
                                    { 101, {83,94}, 1, 372, 0, 372 },
                                    { 102, {84,95}, 1, 354, 0, 354 },
                                    { 103, {85,25}, 1, 846, 0, 846 },
                                    { 104, {85,96}, 3, 270, 90, 424 },
                                    { 105, {86,97}, 2, 1092, 35, 667 },
                                    { 106, {87,53}, 3, 394, 43, 296 },
                                    { 107, {88,98}, 1, 135, 0, 135 },
                                    { 108, {89,99}, 2, 600, 21, 220 },
                                    { 109, {89,100}, 3, 600, 28, 293 },
                                    { 110, {90,101}, 1, 388, 0, 388 },
                                    { 111, {91,102}, 3, 2245, 15, 588 },
                                    { 112, {92,103}, 1, 420, 0, 420 },
                                    { 113, {93,104}, 1, 380, 0, 380 },
                                    { 114, {94,65}, 2, 613, 90, 963 },
                                    { 115, {95,93}, 3, 386, 90, 606 },
                                    { 116, {97,105}, 1, 558, 0, 558 },
                                    { 117, {98,106}, 1, 749, 0, 749 },
                                    { 118, {98,107}, 2, 546, 35, 334 },
                                    { 119, {99,108}, 3, 600, 21, 220 },
                                    { 120, {100,109}, 2, 600, 28, 293 },
                                    { 121, {101,110}, 2, 1118, 11, 215 },
                                    { 122, {102,111}, 3, 599, 83, 868 },
                                    { 123, {102,112}, 3, 874, 48, 732 },
                                    { 124, {103,27}, 3, 385, 44, 296 },
                                    { 125, {104,113}, 3, 387, 90, 608 },
                                    { 126, {105,114}, 3, 696, 35, 425 },
                                    { 127, {107,115}, 1, 347, 0, 347 },
                                    { 128, {108,116}, 1, 1598, 0, 1598 },
                                    { 129, {109,117}, 1, 443, 0, 443 },
                                    { 130, {110,118}, 2, 2489, 15, 652 },
                                    { 131, {111,119}, 2, 554, 31, 300 },
                                    { 132, {112,120}, 1, 726, 0, 726 },
                                    { 133, {113,121}, 1, 929, 0, 929 },
                                    { 134, {114,122}, 1, 730, 0, 730 },
                                    { 135, {115,123}, 3, 298, 35, 182 },
                                    { 136, {116,64}, 3, 349, 90, 548 },
                                    { 137, {116,124}, 1, 323, 0, 323 },
                                    { 138, {117,125}, 1, 446, 0, 446 },
                                    { 139, {118,126}, 2, 1299, 22, 499 },
                                    { 140, {119,127}, 1, 1198, 0, 1198 },
                                    { 141, {120,128}, 2, 940, 35, 574 },
                                    { 142, {121,39}, 3, 387, 90, 608 },
                                    { 143, {121,129}, 1, 1023, 0, 1023 },
                                    { 144, {122,101}, 2, 1118, 37, 722 },
                                    { 145, {123,130}, 1, 1205, 0, 1205 },
                                    { 146, {124,82}, 1, 1116, 0, 1116 },
                                    { 147, {125,131}, 1, 216, 0, 216 },
                                    { 148, {126,132}, 2, 1149, 69, 1384 },
                                    { 149, {127,76}, 3, 386, 90, 606 },
                                    { 150, {128,133}, 1, 559, 0, 559 },
                                    { 151, {129,134}, 1, 23, 0, 23 },
                                    { 152, {130,24}, 3, 270, 90, 424 },
                                    { 153, {131,135}, 2, 600, 35, 367 },
                                    { 154, {132,136}, 3, 906, 70, 1107 },
                                    { 155, {133,137}, 3, 848, 35, 518 },
                                    { 156, {134,28}, 3, 387, 90, 608 },
                                    { 157, {134,51}, 1, 1000, 0, 1000 },
                                    { 158, {135,124}, 3, 600, 35, 367 },
                                    { 159, {136,138}, 1, 75, 0, 75 },
                                    { 160, {137,139}, 1, 287, 0, 287 },
                                    { 161, {138,8}, 2, 613, 90, 963 },
                                    { 162, {138,50}, 3, 387, 90, 608 },
                                    { 163, {139,140}, 3, 453, 90, 712 },
                                    { 164, {140,141}, 1, 740, 0, 740 },
                                    { 165, {141,142}, 2, 642, 90, 1008 },
                                    { 166, {142,143}, 1, 118, 0, 118 },
                                    { 167, {143,129}, 3, 387, 90, 608 },
                                    { 168, {144,145}, 3, 447, 62, 484 },
                                    { 169, {145,146}, 1, 504, 0, 504 },
                                    { 170, {146,147}, 3, 278, 28, 136 },
                                    { 171, {147,148}, 3, 278, 90, 437 },
                                    { 172, {148,109}, 1, 127, 0, 127 },
                                    { 173, {149,130}, 3, 524, 49, 448 }, 
                                    { 174, {96,150}, 1, 1253, 0, 1253 }, 
                                    { 175, {151,152}, 1, 64, 0, 64 }, 
                                    { 176, {152,144}, 2, 370, 48, 310 } };

//定义停车场拓扑地图的顶点
    //                              { {可到达的顶点}, {对应的边},  Pose{x,y,id}}
    vector<Vertex> parkVertexInfoS{ { {202,214},  {201,202},  Pose(2340,1560,201) },
                                    { {203,205},  {203,204},  Pose(2340,1617,202) },
                                    { {204},  {205},  Pose(2185,1934,203) },
                                    { {223},  {222},  Pose(2029,2250,204) },
                                    { {206,212},  {206,207},  Pose(2340,1799,205) },
                                    { {207,209},  {208,209},  Pose(2340,2061,206) },
                                    { {208},  {210},  Pose(2185,2378,207) },
                                    { {224},  {223},  Pose(2029,2694,208) },
                                    { {210,216},  {211,212},  Pose(2340,2189,209) },
                                    { {211},  {213},  Pose(2840,2689,210) },
                                    { {0},  {0},  Pose(2928,2689,211) },
                                    { {213},  {214},  Pose(2940,2399,212) },
                                    { {0},  {0},  Pose(3335,2399,213) },
                                    { {215},  {215},  Pose(2940,2161,214) },
                                    { {0},  {0},  Pose(3335,2161,215) },
                                    { {217,220},  {216,217},  Pose(2340,2285,216) },
                                    { {218},  {218},  Pose(2340,2523,217) },
                                    { {219},  {219},  Pose(2940,1923,218) },
                                    { {0},  {0},  Pose(3335,1923,219) },
                                    { {221},  {220},  Pose(2940,1685,220) },
                                    { {0},  {0},  Pose(3335,1685,221) },
                                    { {201},  {221},  Pose(2437,1812,222) },
                                    { {0},  {0},  Pose(2029,2358,223) },
                                    { {0},  {0},  Pose(2029,2802,224) } };

//定义拓扑地图的边    
    //                          { id, {起点，终点}，边属性1，边属性2，边属性3，边属性4，边属性5 } 
    vector<Edge> parkEdgeInfoS { { 201,  {201,202}, 1, 55, 0, 55, true},
                                { 202,  {201,214}, 3, 600, 90, 942, true },    
                                { 203,  {202,203}, 2, 400, 52, 363, true },
                                { 204,  {202,205}, 1, 182, 0, 182, true },
                                { 205,  {203,204}, 3, 400, 52, 363, true },
                                { 206,  {205,206}, 1, 262, 0, 262, true },
                                { 207,  {205,212}, 3, 600, 90, 942, true },
                                { 208,  {206,207}, 2, 400, 52, 363, true },
                                { 209,  {206,209}, 1, 128, 0, 128, true },
                                { 210,  {207,208}, 3, 400, 52, 363, true },
                                { 211,  {209,210}, 3, 500, 90, 785, true },
                                { 212,  {209,216}, 1, 95, 0, 95, true },
                                { 213,  {210,211}, 1, 12, 0, 12, true },
                                { 214,  {212,213}, 1, 275, 0, 275, true },
                                { 215,  {214,215}, 1, 275, 0, 275, true },
                                { 216,  {216,217}, 1, 238, 0, 238, true },
                                { 217,  {216,220}, 3, 600, 90, 942, false },
                                { 218,  {217,218}, 3, 600, 90, 942, false },
                                { 219,  {218,219}, 1, 275, 0, 275, false },
                                { 220,  {220,221}, 1, 275, 0, 275, false },
                                { 221,  {222,201}, 3, 370, 42, 271, false },
                                { 222,  {204,223}, 1, 108, 0, 108, true },
                                { 223,  {208,224}, 1, 108, 0, 108, true } };
                                
                                
//******************************
//          功能函数
//******************************  
    const vector<string>* getVehicleInfoS()             //获取车辆基本信息   
    {
        return &vehicleInfoS;
    }       

    const vector<vector<double>>* getTLInfoS()          //获取信号灯相关信息
    {
        return &TLInfoS;
    }    

    const vector<Vertex>* getMapVertexInfoS()           //获取道路节点数据
    {
        return &mapVertexInfoS;
    }
    const vector<Edge>* getMapEdgeInfoS()               //获取道路边数据
    {
        return &mapEdgeInfoS;
    }

    const vector<Vertex>* getParkVertexInfoS()          //获取停车场节点数据
    {
        return &parkVertexInfoS;
    }
    const vector<Edge>* getParkEdgeInfoS()              //获取停车场边数据
    {
        return &parkEdgeInfoS;
    }
    
    
